"""
This is part of Human Robot Dynamics-Control, By Tourists.

Based on "OpenLoong Dynamics Control"
Transfer from mujoco to ISAAC SIM by IsaacLab

"""

class Bezier_1D():
    def __init__(self):
        self.P = []
     
    ## positive integer required    
    def factorial(self, num):
        res = 1.0
        if (num<0):
            num=0
        while (num!=0):
            res = res*num
            num -= 1
        return res
    
    ## positive integer required
    def nchoosek(self, n, k):
        res = 1.0
        res= self.factorial(n)/(self.factorial(n-k)* self.factorial(k))
        return res

    def getOut(self, s):
        res = 1.0
        Np = len(self.P)
        for i in range(Np):
            res+= self.nchoosek(Np-1,i)*pow(1-s,Np-1-i)*pow(s,i)*self.P[i]
        return res


